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Abstract 


Many  cart-  and  vehicular-based  unexploded  ordnance  (UXO)  detection  systems  employ  global 
positioning  system  (GPS)  receivers  to  accurately  detennine  the  system’s  position.  However,  the 
unevenness  of  the  terrain  often  causes  the  system  to  tilt  during  the  data  collection,  introducing 
errors  in  the  GPS  measurements.  In  this  work,  three  approaches  are  considered  to  correct  the 
errors  in  the  GPS  measurements  caused  by  the  tilting  of  the  system:  low-pass  filtering  (LPF), 
linear  predictive  filtering,  and  adaptive  filtering  using  a  hidden  Markov  model  (HMM).  The  LPF 
smooths  the  data  collection  path  recorded  by  the  GPS  receiver.  Although  this  filter  does  not 
explicitly  model  the  system  motion,  it  does  remove  dramatic,  and  unrealistic,  jumps  in  the  GPS 
measurements.  In  contrast,  the  movement  of  the  system  can  be  explicitly  modeled  by  an  HMM. 
The  HMM  characterizes  the  cart  motion  so  that  the  subsequent  filtering  is  appropriate  for  the 
type  of  motion  encountered.  The  error  correction  techniques  are  first  applied  to  simulated  data, 
in  which  both  the  sources  of  error  and  the  ground  truth  are  known  so  that  the  perfonnance  of  the 
algorithms  can  be  compared.  The  algorithms  are  then  applied  to  measured  data  collected  with  a 
cart-based  system  to  evaluate  the  robustness  of  their  performance.  Although  the  HMM  approach 
is  found  to  represent  an  improvement  over  the  LPF,  robustness  issues  remain  with  this  approach 
-  mainly  through  ambiguities  in  the  cart  orientation.  A  quaternion  formulation  is  therefore 
proposed  as  a  possible  efficient  means  to  track  and  process  the  cart  orientation  as  it  moves  over 
the  uneven  terrain. 
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1.  Introduction 


The  discrimination  of  unexploded  ordnance  (UXO)  from  surrounding  metallic  clutter  poses  a 
great  technological  challenge.  One  approach  that  has  shown  promise  in  addressing  this  challenge 
makes  use  of  the  following  important  observation:  most  electromagnetic  induction  (EMI) 
signatures  of  UXO  and  clutter  are  very  well  characterized  by  a  simple  induced  dipole  response 
model  expressed  by  the  eigenvalues  of  the  magnetic  polarizability  tensor  along  the  object’s  three 
principal  axes  [1-3].  To  the  extent  that  these  eigenvalues  differ  between  UXO  and  clutter, 
discrimination  is  possible. 

Results  of  inverting  controlled  EMI  measurements,  over  ordnance  and  clutter,  have 
demonstrated  that  a  high  degree  of  discrimination  is  realizable  [4-5],  However,  when  applied  to 
field  data  -  where  the  sensor  positions  are  generally  not  precisely  known  -  the  same  level  of 
success  has  not  been  achieved  [6].  The  reason  is  simple:  because  signal  amplitude  depends 
strongly  on  range,  positioning  errors  can  cause  significant  deviations  in  the  signal  calculations 
used  in  inverting  the  data.  This  seriously  degrades  the  ability  to  capture  the  subtle  spatial 
variations  of  the  signal  that  are  crucial  to  discriminating  between  different  types  of  objects. 

The  goal  of  this  work  is  to  correct  errors  in  global  positioning  system  (GPS)  measurements 
recorded  by  a  cart-mounted  UXO  detection  system.  The  UXO  detection  system  includes  EMI 
and  magnetometer  sensors  and  a  GPS  receiver,  intended  to  accurately  determine  the  position  of 
the  system,  mounted  above  the  cart.  Unfortunately,  in  many  instances,  the  unevenness  of  the 
ground  causes  the  system  to  tilt.  When  the  system  tilts,  the  recorded  GPS  measurements  are 
offset  from  the  true  system  positions  and  discrimination  performance  is  consequently 
diminished. 

The  GPS  measurement  error-correction  algorithms  presented  in  this  report  are  intended  to  reduce 
the  errors  in  the  GPS  measurements  introduced  by  the  tilting  of  the  cart.  Three  approaches  are 
considered:  low-pass  filtering  (LPF),  linear  predictive  filtering,  and  adaptive  filtering  using  a 
hidden  Markov  model  (HMM).  The  LPF  is  the  baseline  error  correction  algorithm,  and  removes 
dramatic  and  unrealistic  jumps  in  the  GPS  measurements  even  though  it  does  not  explicitly 
model  the  system  motion.  The  linear  predictive  filter  offers  a  causal  approach,  but  again  does  not 
explicitly  model  the  system  motion.  The  third  approach  -  the  HMM  approach  -  does  explicitly 
include  the  system  movement.  In  this  approach,  the  cart  motion  is  broadly  characterized  as  either 
linear  or  nonlinear,  after  which  an  appropriate  filter  is  applied. 

All  three  error  correction  techniques  are  applied  to  simulated  data  -  for  which  both  the  sources 
of  error  and  the  ground  truth  are  known  -  so  that  the  performance  of  the  algorithms  can  be 
compared.  The  algorithms  are  then  applied  to  measured  data  collected  with  a  cart-based  system 
at  the  Advanced  Technology  Demonstration  at  Jefferson  Proving  Ground  (ATD/JPG)  to  evaluate 
their  robustness  under  real  conditions. 

Finally,  in  order  to  address  deficiencies  in  the  HMM  approach,  a  quaternion  formulation  is 
proposed  as  a  possible  means  of  tracking  and  processing  the  cart  orientation  as  it  moves  over  the 
uneven  terrain.  The  addition  of  this  information  is  crucial  for  resolving  ambiguities  encountered 
when  attempting  to  characterize  the  motion  as  either  linear  or  nonlinear,  and  offers  an 
opportunity  for  improved  robustness. 
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The  accomplishments  include: 

•  Modeling  of  cart  path,  cart  motion,  GPS  measurement  errors,  and  sensors. 

•  Development  of  algorithms  for  GPS  measurement  error-correction  and  cart  motion 
characterization. 

•  Analysis  of  GPS  error  correction  algorithms  through  simulation. 

•  Application  of  GPS  error  correction  algorithms  to  data  collected  at  ATD/JPG. 

•  Modeling  of  cart  orientation  using  a  quaternion  formulation. 


2.  Models 

2.1.  Cart  Path  Models 

2.1.1.  Idealized  Cart  Path  Models 

Several  idealized  cart  motion  models,  which  are  intended  to  reveal  the  effects  of  curvature  of  the 
cart  path  on  algorithm  performance,  are  considered  in  the  simulations.  They  are  shown  in  Figure 
1.  For  each  of  the  cart  paths,  the  distance  between  successive  cart  locations  is  1  unit.  Since  there 
are  500  measurements  in  each  path,  each  path  covers  a  distance  of  499  units. 

The  straight  line  path  and  circular  paths,  shown  in  subplots  (a)  through  (f),  are  paths  which  have 
a  constant  curvature  over  the  entire  path.  The  straight  line  has  curvature  k  =  0 ,  and  the  arcs  have 

curvature  k  =  — ,  where  a  is  the  radius  of  the  circle.  The  sinusoidal  paths,  shown  in  subplots  (g) 
a 

through  (k),  have  varying  curvature  over  the  entire  path.  For  these  paths,  the  curvature  is 
calculated  numerically.  When  the  cart  path  is  expressed  in  vector  notation,  P(t)  =  x(t)i  +  y(t) j , 
the  curvature  is 


|#- 

v 

where  \(t)  =  P (t) .  The  curvature  of  the  sinusoidal  paths  is  periodic,  with  a  period  twice  that  of 
the  underlying  sinusoidal  curve.  This  is  illustrated  in  Figure  2,  which  shows  the  curvature  of  the 
path  consisting  of  3  sinusoid  cycles.  The  simulations  presented  in  Sec.  4.3  show  that  not  only 
does  the  curvature  of  the  path  affect  algorithm  perfonnance,  but  it  is  also  related  to  the  mean 
RMS  error  in  distance. 
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(d)  1/2  Circle 


(e)  3/4  Circle 


(f)  Full  Circle 


(g)  1  Sinusoid  Cycle  (h)  2  Sinusoid  Cycles  (i)  3  Sinusoid  Cycles 


(j)  4  Sinusoid  Cycles  (k)  5  Sinusoid  Cycles 

Figure  1:  Cart  paths  considered  in  the  simulation  studies. 
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(a)  3  Sinusoid  Cycles  (b)  Curvature  of  3  Sinusoid  Cycles 

Figure  2:  Curvature  of  the  path  consisting  of  3  Sinusoid  Cycles. 


2.1.2.  Realistic  Cart  Path  Model 

A  realistic  cart  path  model  is  depicted  in  Figure  3.  The  parameters  for  the  path  were  chosen  to  be 
consistent  with  the  sampling  parameters  of  existing  cart  and  handheld  systems.  The  distance 
between  successive  down-track  measurements  is  5cm,  and  the  distance  between  adjacent  cross¬ 
track  passes  is  0.5m.  For  the  purpose  of  describing  the  path,  the  passes  are  numbered  from 
bottom  to  top,  so  pass  1  is  at  0m  in  the  y-direction  and  pass  6  is  at  2.5m  in  the  y-direction.  The 
small  bumps  in  the  path,  located  around  x=lm  (passes  2-5)  and  x=4.2m  (passes  3-6)  in  the  x- 
direction,  are  intended  to  simulate  the  perturbations  in  the  GPS  measurements  induced  by  tilting 
of  the  cart.  In  addition,  detours  around  obstacles  are  simulated  between  2.5  and  3m  in  the  x- 
direction  in  passes  2  and  3,  and  between  2  and  3m  in  the  x-direction  in  passes  5  and  6.  Finally, 
the  turns  between  passes  are  modeled  as  half-circles. 

2.2.  Cart  Motion  Model 

The  hidden  Markov  model  (HMM)  uses  the  GPS  measurements  to  characterize  the  cart  path  as 
either  moving  forward,  detour  to  the  left,  detour  to  the  right,  or  turn  around,  and  these  states  are 
defined  for  each  of  the  pass  directions.  The  model  is  depicted  in  Figure  4.  In  this  model,  the 
observable  is  the  change  in  the  cart  position  from  one  measurement  to  the  next.  Thus,  when  the 
model  is  in  a  particular  state,  that  state  defines  the  movement  that  should  occur  between  the 
current  measurement  and  the  next  measurement.  For  instance,  when  the  model  is  in  the  right 
forward  motion  state,  the  next  measurement  should  be  5  cm  to  the  right  of  the  current 
measurement. 
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y-Coordinate  [m] 


Simulated  GPS  Measurements 


Figure  3:  Simulated  error-free  GPS  measurements. 


Figure  4:  Flidden  Markov  model  for  characterizing  the  cart  path. 
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Each  of  the  detour  states  is  further  divided  into  5  states.  The  first  two  states  are  the  movement 
away  from  the  ideal  straight  line  path,  the  third  state  is  movement  parallel  to  the  ideal  straight 
line  path,  and  the  last  two  states  are  movement  back  toward  the  ideal  straight  line  path.  There  are 
two  states  moving  away  from  and  back  toward  the  ideal  path  so  that  detours  are  forced  to  be  at 
least  2  measurements  away  from  the  ideal  path.  Using  5  states,  rather  than  3,  eliminated  many 
errors  in  the  estimated  state  sequence  where  perturbations  in  the  GPS  measurements  cause 
straight  line  segments  to  be  characterized  as  small  detours.  If  the  characteristics  of  the  detours 
suggest  it  would  be  appropriate,  more  states  can  be  added  to  the  detour  model  to  force  the  detour 
to  be  farther  away  from  the  ideal  path.  A  model  in  which  the  detours  were  characterized  by  only 
3  states  was  also  considered,  and  the  simulations  showed  that  the  5-state  detour  model  was  more 
robust  than  the  3-state  detour  model  when  perturbations  in  the  GPS  measurements  were  caused 
by  cart  tilt.  Therefore,  it  is  anticipated  that  forcing  the  detours  to  be  larger  would  eliminate  more 
errors  in  the  estimated  state  sequence  where  perturbations  in  the  GPS  measurements  cause 
straight  line  segments  to  be  characterized  as  detours.  It  is  likely  that  parameters,  such  as  the 
detour  size,  will  be  site  dependent. 

Each  of  the  turn  around  states  are  also  divided  into  several  states,  and  are  modeled  as  known 
movement  in  a  circular  path.  For  the  chosen  simulation  parameters  (5  cm  between  measurements 
and  0.5m  between  passes),  there  are  16  states  in  each  turn  around. 

2.3.  GPS  Measurement  Error  Models 

The  GPS  measurements  are  modeled  as  jointly  Gaussian.  Both  uncorrelated  and  correlated  errors 
are  considered,  and  the  effects  of  correlation  in  the  measurement  errors  are  evaluated  through 
simulations.  We  believe  that  assuming  a  jointly  Gaussian  model  will  allow  us  to  work  on  the 
error  correction  algorithms,  and  also  provide  the  flexibility  to  determine  the  model  parameters 
from  the  actual  ground  surface  and  cart  dynamics  at  a  later  time. 

2.3.1.  Uncorrelated  Errors 

Initially,  the  GPS  measurement  errors  are  modeled  as  uncorrelated  jointly  Gaussian  random 
variables.  An  example  trajectory  (solid  line)  and  the  corresponding  GPS  measurements  (open 
circles)  are  shown  in  Figure  5.  In  this  example,  the  simulated  cart  path  is  described  by  y=x,  and 
the  GPS  measurement  errors  are  uncorrelated  Gaussian  random  variables  with  variance  cT  =  0.1. 
The  degree  of  error  shown  here  is  considered  to  be  moderate  for  the  simulations.  The  simulations 
also  investigate  both  larger  and  smaller  error. 

2.3.2.  Correlated  Errors 

The  GPS  measurement  error  model  is  extended  to  include  correlation  between  errors  in  the  x- 
and  y-coordinates.  Thus,  the  model  now  assumes  the  measurements  in  the  two  coordinate 
directions  are  jointly  Gaussian  with  some  correlation.  The  correlated  Gaussian  random  variables 
were  computed  by  post-multiplying  uncorrelated  Gaussian  random  variables  by  the  upper 
triangular  Cholesky  decomposition  of  the  desired  correlation  matrix,  R ,  where 
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2D  Gaussian  RV  for  position  error 


Figure  5:  Example  cart  trajectory  (solid  line)  and  corresponding  GPS  measurements  (open 
circles)  for  an  uncorrelated  jointly  Gaussian  GPS  measurement  error  model. 


R  = 


1 

r 


In  this  notation,  the  cross-correlation  between  the  errors  in  the  x-  and  y-coordinates  is  denoted 
by  r. 

The  effects  of  correlation  in  the  errors  are  illustrated  in  Figure  6.  This  figure  shows  an  example 
cart  track  and  the  corresponding  GPS  measurements  for  uncorrelated  (r  =0),  moderately 
correlated  (r  =0.5)  and  highly  correlated  (r  =  0.9)  GPS  measurement  errors  for  a  moderate  error 
level  (i.e.  variance  <j~  =  0.1).  These  examples  show  that  even  though  the  variances  of  the  errors 
remain  constant,  the  degree  of  variability  appears  to  be  dependent  on  the  correlation.  The  more 
correlated  the  errors  are,  the  less  variable  they  appear  to  be.  The  simulations  presented  in  Sec. 
4.2  show  that  the  degree  of  correlation  does  not  affect  the  performance  of  the  algorithms 
evaluated  in  these  simulations. 
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(a)  Uncorrelated  (r  =  0) 


x-Coordlnate 

(b)  Moderately  correlated  (/■  =  0.5) 


x-Coordmate 

(c)  Highly  correlated  ( r  =  0.9) 


Figure  6:  Example  cart  path  (solid  line)  and  corresponding  GPS  measurements  (open  circles)  for 
uncorrelated,  moderately  correlated,  and  highly  correlated  GPS  measurement  errors. 


2.4.  Sensor  Models 

The  EMI  dipole  and  magnetometer  dipole  models  and  parameter  estimation  algorithms  used  in 
these  simulations  [7]  are  the  same  as  those  used  for  the  vehicular  systems,  with  the  appropriate 
modifications  made  to  make  them  consistent  with  the  man-portable  or  handheld  system  sampling 
geometries.  The  model  parameters  are  estimated  from  measured  data,  and  the  resulting  estimates 
utilized  for  UXO  detection  and/or  discrimination.  Thus,  it  is  desirable  to  have  accurate  parameter 
estimates. 

2.4.1.  Electromagnetic  Induction 

The  EMI  dipole  model  is  intended  to  replicate  the  EM61  response.  There  are  5  parameters  in  the 
EMI  dipole  model:  the  dipole  which  is  composed  of  the  two  parameters  mi  and  m2 
( m  =  [mi  in ,  m2]),  the  target  inclination  (0),  the  target  azimuth  (c()m),  and  the  target  depth  (cl). 

The  dipole  is  a  function  of  the  physical  properties  of  the  target.  For  UXO,  im  >  m  \ ,  and  generally 

YYl 

2  <  — -  <  10  .  The  target  inclination  is  defined  as  the  angle  from  the  vertical  axis  (z)  through  the 

/«, 

target  center  to  the  principle  axis  of  the  target.  Thus,  0  =  0  corresponds  to  a  vertical  target,  and 
0=7t/2  corresponds  to  a  horizontal  target.  The  target  azimuth  is  defined  (in  the  x-y  plane)  as  the 
angle  from  the  projection  of  the  principle  axis  of  the  target  to  the  x-axis.  If  the  cart  is  traveling 
parallel  to  the  y-axis,  <\>m=  0  describes  a  target  lying  perpendicular  to  the  direction  of  travel,  and 
c()m=7r/2  describes  a  target  lying  parallel  to  the  direction  of  travel. 

2.4.2.  Magnetometer 

The  magnetometer  dipole  model  has  3  parameters:  the  target  inclination  (0),  the  target  azimuth 
((j)m),  and  the  target  depth  (cl).  The  definitions  of  the  target  inclination  and  the  target  azimuth  are 
the  same  as  for  the  EMI  dipole  model. 
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3.  Algorithms 

3.1.  GPS  Measurement  Filters 
3.1.1.  Low  Pass  Filters 

The  baseline  GPS  measurement  error  correction  algorithm  is  a  simple  low-pass  filter  (LPF). 
Although  this  approach  will  reduce  large  jumps  in  the  GPS  measurements,  this  algorithm  does 
not  incorporate  models  of  either  the  cart  motion  or  the  ground  surface.  Several  approaches  were 
taken  in  designing  the  LPF.  Since  a  causal  filter  results  in  a  lag  between  the  actual  measurements 
and  the  filtered  result,  only  non-causal  filters  were  considered.  In  addition  to  a  simple  moving 
average  filter,  other  filters  which  weight  the  data  unequally  were  also  considered.  The  effects  of 
filter  order  were  evaluated  for  each  of  the  filters. 

The  moving  average  filter  is  a  simple  arithmetic  average  of  the  k+ 1  (odd)  points  about  the  point 
of  interest,  where  k  is  the  filter  order.  The  impulse  response  is  a  rectangular  function,  and  the 
frequency  response  is  low-pass.  Both  are  shown  in  Figure  7(a)  for  a  filter  order  of  6. 

The  modified  moving  average  filters  are  similar  to  the  moving  average  filter,  except  the  tap 
weights  emphasize  the  points  near  the  point  of  interest  rather  than  weighting  all  the  points 
equally.  The  windowing  functions  considered  were  the  triangular  and  Hamming  windows.  The 
impulse  functions  and  the  corresponding  low-pass  frequency  responses  for  a  filter  order  of  6  are 
shown  in  Figures  7(b)  and  7(c),  respectively.  Both  of  these  filters  have  a  broader  frequency 
response  than  a  rectangular  filter  of  the  same  length.  As  expected,  the  simulation  results  show 
that  the  rectangular  filter,  which  has  the  narrowest  response,  has  better  error-correction 
performance  than  either  of  the  other  filters. 


0.2 

0.15 

0.1 

0.05 
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Rectangular  Impulse  Response 


-2  0  2 
Magnitude  of  Frequency  Response 


0.2  0.4  0.6  08 

Normalized  Frequency  (xh  rad/sample) 

(a)  Rectangular 


Triangular  Impulse  Response 


Hamming  Impulse  Response 


(b)  Triangular 


(c)  Hamming 


Figure  7:  Impulse  and  frequency  responses  of  the  low-pass  filters. 
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3.1.2.  Linear  Predictive  Filter 


A  linear  predictive  filter  [8]  has  also  been  implemented  for  GPS  measurement  error  correction. 
Linear  prediction  differs  from  the  LPF  in  that  it  provides  a  causal  solution  to  error-correcting  the 
GPS  measurements.  This  method  assumes  a  linear  model,  z  =  can  +  ft ,  where  z  is  the  coordinate 
of  the  cart,  either  x  or  y,  and  m  is  the  measurement  number. 

Linear  prediction  seeks  to  minimize  the  expected  value  of  the  squared  error  between  the  assumed 
linear  model  and  the  GPS  measurement,  zGPS,  which  again  is  cither  xGps  or  yGps, 

£  —  E\(zGps  —  z)  ]  . 

Setting  the  partial  derivatives  of  £  with  respect  to  both  a  and  /3  equal  to  zero,  yields  two 
equations  in  two  unknowns.  The  solution  to  this  set  of  equations,  denoted  by  ao  and  f:ku  is 

_COV[m,zGPS\  _pc t/gps 
U0  ~  2  ~ 


and 


P()  A GPS  a0m  ■ 

Thus,  the  parameters  for  the  linear  model,  ao  and  j3o,  are  determined  from  the  data,  and  the 
estimate  of  the  current  coordinate  is  calculated  from  the  model  by  z  =  a0m  +  J30 . 

As  applied  to  this  problem,  the  linear  predictive  filter  of  order  k  uses  the  previous  k  GPS 
measurements  as  well  as  the  current  measurement,  for  a  total  of  k  +1  points,  to  estimate  the 
current  coordinate. 

3.2.  Cart  Motion  Characterization 

The  cart  motion  state  sequence  -  from  which  the  cart  motion  is  characterized  as  linear  or 
nonlinear  -  is  estimated  using  the  Viterbi  algorithm  [9],  The  Viterbi  algorithm  is  a  useful  method 
for  finding  the  maximum  a  posteriori  (MAP)  estimate  of  the  state  sequence  of  a  finite-state 
discrete-time  Markov  process  observed  in  memory-less  noise  when  the  output  of  the  process  is  a 
function  of  its  current  state.  It  accomplishes  this  by  finding  the  state  sequence  which  maximizes 
the  conditional  a  posteriori  probability  of  the  state  sequence  given  the  noisy  observations. 

For  a  system  with  S  states  and  O  observations,  the  number  of  possible  states  sequences  is  S°. 
Thus,  the  computational  intensity  grows  exponentially  with  the  number  of  observations  if  the 
conditional  a  posteriori  probability  is  calculated  for  every  possible  state  sequence.  However,  the 
Viterbi  algorithm  reduces  the  computational  intensity  to  linear  growth  by  detennining  the  most 
likely  state  sequence  leading  to  every  possible  ending  state  after  each  observation.  In  this  way, 
after  every  observation  the  number  of  remaining  state  sequences  is  equal  to  the  number  of 
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possible  ending  states,  and  the  search  space  for  the  next  observation  is  greatly  reduced.  This 
remains  an  optimal  solution  to  the  maximization  problem  because  the  most  likely  state  sequence 
leading  to  any  state  after  observation  o  must  include  one  of  the  most  likely  state  sequences 
determined  after  observation  o- 1.  Thus,  the  Viterbi  algorithm  naturally  lends  itself  to  a 
sequential  implementation. 

3.3.  Sensor  Model  Parameter  Estimation 

The  sensor  model  parameters  are  estimated  from  measured  data,  and  the  resulting  estimates  used 
for  UXO  detection  and/or  discrimination.  For  the  simulations  investigating  the  effects  of  GPS 
positional  errors  on  model  parameter  estimation,  noise-free  data  is  first  simulated  on  a  perfect 
grid,  simulating  the  sampling  intervals  of  the  handheld  system.  In  all  the  simulations,  the  data  is 
generated  assuming  that  a  pass  is  made  directly  over  the  center  of  the  target.  After  the  simulated 
data  is  computed,  the  coordinates  of  the  grid  are  perturbed  to  simulate  GPS  measurement  errors. 
The  parameters  for  the  sensor  model  are  then  estimated  from  the  simulated  sensor  data  and  the 
perturbed  coordinates. 

The  parameter  inversion  process  is  illustrated  in  Figure  8  using  EMI  sensor  data.  For  this 
example,  the  EMI  dipole  model  parameters  are  m  =  [10  10  20],  0  =  n/2  ,  <\>m  =  n/2,  and  d  = 
0.75m.  The  measurements  are  taken  at  0.05m  intervals  in  the  down-track  direction  and  0.5m 
intervals  in  the  cross-track  direction.  The  data  is  displayed  with  the  original  sample  locations 
(denoted  by  ‘x’)  and  the  perturbed  sample  locations  (denoted  by  ‘o’)  superimposed  in  subplots 
(a)  and  (b),  respectively,  for  a  moderate  error  level  (a  =10'  m  in  total  distance).  The  data, 
which  is  assumed  to  have  been  measured  at  the  irregularly  spaced  points  shown  in  subplot  (b),  is 
then  interpolated  to  a  perfect  grid,  as  shown  in  subplot  (c).  The  dipole  parameters  are  estimated 
from  the  interpolated  data,  and  simulated  data  generated  using  the  parameter  estimates  is  shown 
in  subplot  (d).  In  this  example,  the  estimated  EMI  dipole  model  parameters  are  m  =  [10.0511 
10.05 1 1  20.0966],  9  =  0.4996tt,  <|>m  =  0.5005tt,  and  d  =  0.75 13m. 


4.  Simulation  Studies 

4.1.  Sensitivity  of  Sensor  Model  Parameter  Estimation 

The  sensitivity  of  the  EMI  and  magnetometer  dipole  models  to  perturbations  in  the  GPS 
measurements  is  evaluated  through  simulations.  The  simulations  are  perfonned  for  a  variety  of 
dipole  model  parameters,  which  are  listed  in  Table  1.  All  5  parameters  apply  to  the  EMI 
simulations;  however,  only  0,  (|)m,  and  d  apply  to  the  magnetometer  simulations.  Therefore,  only 
simulations  1  through  4  are  performed  for  the  magnetometer  model. 

The  RMS  errors  are  computed  from  100  independent  realizations  of  GPS  measurement  errors. 
The  variance  of  the  GPS  measurement  errors,  as  measured  by  total  distance,  ranges  from  10"6  to 
10'3  m2.  The  corresponding  variances  in  the  x-  and  y-coordinates  are  1 0‘6/V2  to  1 0‘3/V2  m2.  The 
dipole  model  parameters  are  estimated  from  each  realization  10  times,  with  the  initial  conditions 
selected  randomly  for  each  estimation  in  order  to  avoid  the  problem  of  convergence  at  a  local 
minimum,  rather  than  a  global  minimum.  The  estimated  parameters  for  each  realization  are 
selected  by  choosing  the  estimation  results  which  result  in  the  lowest  error.  In  all  the 
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simulations,  the  simulated  sensor  data  is  noise-free.  In  addition,  the  realizations  which  fail  to 
converge  at  the  global  minimum  are  removed  from  the  data  before  the  RMS  errors  are  computed. 
In  all  cases,  at  least  98  realizations  are  retained  to  calculate  the  RMS  error. 


Simulation 

m\ 

mi 

e 

<t>m 

d 

1 

10 

20 

0.57T 

0.5n 

0.75m 

2 

10 

50 

0 

0 

0.75m 

3 

10 

50 

0 

0.57T 

0.75m 

4 

10 

50 

0.5/r 

0 

0.75m 

5 

10 

50 

0.5tt 

0.57T 

0.75m 

6 

10 

100 

0.5n 

0.5tt 

0.75m 

Table  1:  EMI  and  magnetometer  dipole  model  simulation  parameters. 


x-coonJinate  |m| 


(a)  Actual  Measurement  Locations 


(b)  Perturbed  Measurement  Locations 


x-coon»«*t*  |m) 


« -coordinate  |m) 


(c)  Gridded  Measurement 


(d)  Estimated  Parameter  Simulation 


Figure  8:  Example  EMI  dipole  model  parameter  estimation  with  perturbed  GPS  measurements.  The  actual 
model  parameters  are  mx  =  10,  »i2  =  20,  0  =  0.571,  (|>„,  =  0.57t,  and  d  =  0.75m,  and  the  estimated  model 
parameters  are  mx  =  10.0511,  »i2  =  20.0966,  0  =  0.499671,  (|>,„  =  0.50  0571,  and  d  =  0.7513m. 
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Figures  9  and  10  show  the  RMS  errors  in  the  EMI  and  magnetometer  parameter  estimates  as  a 
function  of  the  GPS  measurement  error  variance,  measured  in  distance,  for  each  of  the 
simulations.  As  expected,  the  results  show  that  decreasing  the  variance  in  the  GPS  measurement 
errors  tends  to  reduce  the  RMS  error  in  the  dipole  model  parameter  estimates.  As  this  set  of 
simulations  illustrates,  the  improvement  in  the  parameter  estimates  depends  on  both  the  target 
dipole  parameters  and  the  orientation  of  the  target  relative  to  the  data  collection.  For  example, 
the  RMS  error  in  the  estimate  of  c()m  (target  azimuth)  is  independent  of  the  GPS  measurement 
error  variance  for  simulations  2,  3,  and  6,  and  decreases  with  the  GPS  measurement  error 
variance  for  simulations  1,  4,  and  5. 

4.2.  Effects  of  GPS  Measurement  Error  Correlation 

The  effects  of  correlation  in  the  measurement  errors  are  investigated  through  simulations  for  the 
straight  line  and  half  circle  cart  paths  at  all  three  error  levels.  The  mean  RMS  errors  as  a  function 
of  filter  order  for  both  the  x-  and  y-coordinates  as  well  as  overall  distance  are  shown  in  Figures 
11  through  16.  The  results  in  these  figures  show  that  for  the  algorithms  considered  here,  the 
mean  RMS  error  is  not  a  function  of  the  measurement  error  correlation.  This  result  is  not 
surprising  since  the  current  algorithms  operate  on  the  x-  and  y-coordinate  data  separately.  It  is 
anticipated  that  the  correlation  will  affect  the  performance  of  future  algorithms  which  operate  on 
the  two  coordinates  simultaneously.  It  is  interesting  to  note  that  the  half  circle  cart  path  results 
show  that  higher  filter  order  does  not  necessarily  provide  better  perfonnance.  This  is  due  to  the 
curvature  of  the  cart  path,  which  is  discussed  in  the  following  section. 
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Figure  9:  RMS  errors  in  the  EMI  dipole  model  parameter  estimates  as  a  function  of  the  GPS 
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Figure  10:  RMS  errors  in  the  magnetometer  dipole  model  parameter  estimates  as  a  function  of  the  GPS 
measurement  error  variance  (measured  in  distance)  for  all  the  simulations. 
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Figure  11:  RMS  Error  as  a  function  of  filter  order  for  the  straight  line  cart  path  with  low  measurement 

error  (ct2  =  0.01). 
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Figure  12:  RMS  Error  as  a  function  of  filter  order  for  the  straight  line  cart  path  with  moderate 

measurement  error  (ct2  =  0.1). 
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Figure  13:  RMS  Error  as  a  function  of  filter  order  for  the  straight  line  cart  path  with  high  measurement 

error  (ct2  =  1). 
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Figure  14:  RMS  Error  as  a  function  of  filter  order  for  the  half  circle  cart  path  with  low  measurement 

error  (a2  =  0.01). 
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Figure  15:  RMS  Error  as  a  function  of  filter  order  for  the  half  circle  cart  path  with  moderate 

measurement  error  (a2  =  0.1). 
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Figure  16:  RMS  Error  as  a  function  of  filter  order  for  the  half  circle  cart  path  with  high  measurement 

error  (a2  =  1). 
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4.3.  Effects  of  Cart  Path  Curvature 


The  effects  of  curvature  in  the  cart  path  are  investigated  through  simulations  for  all  of  the  cart 
paths  at  all  three  error  levels.  Since  the  simulation  results  for  the  straight  line  and  half  circle  cart 
paths  showed  that  correlation  in  the  measurement  errors  does  not  affect  performance  of  the 
current  algorithms,  the  simulations  studying  the  effects  of  curvature  in  the  cart  path  are 
performed  only  for  moderately  correlated  errors  (r  =  0.5).  The  other  levels  of  error  correlation 
should  produce  the  same  results  as  those  shown  here. 

The  mean  RMS  errors  as  a  function  of  filter  order  for  both  the  x-  and  y-coordinates  as  well  as 
overall  distance  are  shown  in  Figures  17  through  27.  These  summary  results  show  that  the  cart 
path  curvature  affects  algorithm  performance.  As  the  path  curvature  increases  (Figures  17-22), 
the  perfonnance  degrades.  This  is  most  noticeable  at  the  low  error  level  (a2  =  0.01).  In  addition, 
it  appears  as  though  cart  path  curvature  also  affects  the  choice  of  filter  order  since  higher  filter 
order  increases  the  algorithm  sensitivity  to  curvature  in  the  cart  path. 

It  is  interesting  that  for  the  set  of  sinusoid  paths  the  x-  and  y-coordinates  often  have  different 
performance  curves.  This,  again,  is  due  to  the  curvature  of  the  cart  path.  When  the  x-  and  y- 
coordinates  are  plotted  versus  measurement  number,  it  can  be  seen  that  x-coordinate  is  more 
linear  with  measurement  number  than  the  y-coordinate.  Hence,  the  algorithms  have  better 
performance  on  the  x-coordinate  than  the  y-coordinate. 

To  gain  a  better  understanding  of  how  the  cart  path  curvature,  filter  order,  and  noise  level 
interact,  it  is  instructive  to  examine  the  results  over  the  entire  cart  path,  rather  than  averaging  the 
results  over  the  measurements.  Results  obtained  for  the  2  sinusoid  cycle  cart  path  are  shown  in 
Figures  28  through  30.  These  figures  show  the  mean  RMS  error  as  a  function  of  measurement 
number.  The  results  indicate  that  when  the  noise  level  is  low  the  RMS  error  fluctuates  with  the 
cart  path  curvature,  so  when  the  curvature  is  large,  the  error  is  large,  and  conversely  when  the 
curvature  is  small,  the  error  is  small.  The  larger  the  filter  order,  the  more  pronounced  this  effect. 
This  occurs  because  at  low  noise  levels,  the  algorithms  are  limited  by  the  accuracy  of  the 
underlying  model,  which  for  these  algorithms  is  linear.  The  linear  assumption  is  least  appropriate 
when  the  path  curvature  is  large,  and  this  produces  the  larger  errors  at  those  places  in  the  path. 
However,  at  high  measurement  error  levels,  the  algorithms  are  noise  limited.  When  the 
measurement  error  is  large,  the  modeling  error  is  small  in  comparison.  Since  these  algorithms 
either  implicitly  (LPF)  or  explicitly  (linear  prediction)  assume  a  linear  cart  path,  and  they  are 
sensitive  to  the  accuracy  of  this  assumption.  Therefore,  they  are  sensitive  to  curvature  in  the 
path,  and  low  error  level  and/or  large  filter  order  increases  this  sensitivity. 

The  interaction  between  the  cart  path  curvature  and  filter  order  for  the  rectangular  LPF  is  shown 
in  Figure  31.  These  results  were  compiled  using  the  straight  line  and  circular  cart  paths,  all  of 
which  have  a  constant  curvature  over  the  entire  cart  path.  These  figures  clearly  show  that  at  a 
low  noise  level,  where  the  modeling  error  dominates,  a  low  filter  order  provides  the  best 
performance,  and  that  at  a  high  noise  level,  where  the  measurement  error  dominates,  a  high  filter 
order  provides  the  best  performance.  However,  at  a  moderate  noise  level,  where  neither  the 
modeling  error  nor  the  measurement  error  is  dominant,  the  best  filter  order  depends  on  the 
curvature  of  the  cart  path. 
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Figure  17:  RMS  Error  as  a  function  of  filter  order  for  the  straight  line  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  18:  RMS  Error  as  a  function  of  filter  order  for  the  1/8  circle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  19:  RMS  Error  as  a  function  of  filter  order  for  the  1/4  circle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  20:  RMS  Error  as  a  function  of  filter  order  for  the  1/2  circle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  21:  RMS  Error  as  a  function  of  filter  order  for  the  3/4  circle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  22:  RMS  Error  as  a  function  of  filter  order  for  the  full  circle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  23:  RMS  Error  as  a  function  of  filter  order  for  the  1  sinusoid  cycle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  24:  RMS  Error  as  a  function  of  filter  order  for  the  2  sinusoid  cycle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  25:  RMS  Error  as  a  function  of  filter  order  for  the  3  sinusoid  cycle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  26:  RMS  Error  as  a  function  of  filter  order  for  the  4  sinusoid  cycle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  27:  RMS  Error  as  a  function  of  filter  order  for  the  5  sinusoid  cycle  cart  path  with  moderate 

measurement  error  correlation  (r  =  0.5). 
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Figure  28:  RMS  Error  as  a  function  of  measurement  number  with  moderate  measurement  error 
correlation  (r  =  0.5)  for  the  2  sinusoid  cycle  cart  path  and  filter  order  of  6. 
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Figure  29:  RMS  Error  as  a  function  of  measurement  number  with  moderate  measurement  error 
correlation  (r  =  0.5)  for  the  2  sinusoid  cycle  cart  path  and  filter  order  of  12. 
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Figure  30:  RMS  Error  as  a  function  of  measurement  number  with  moderate  measurement  error 
correlation  (r  =  0.5)  for  the  2  sinusoid  cycle  cart  path  and  filter  order  of  18. 
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Figure  31:  RMS  Error  produced  by  the  rectangular  low-pass  filter  as  a  function  of  path  curvature 
and  filter  order  for  the  straight  line  and  circular  cart  paths  with  moderate  measurement  error 

correlation  (r  =  0.5). 
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For  cart  paths  which  do  not  have  constant  curvature,  such  as  the  sinusoidal  paths  considered  in 
these  simulations,  the  choice  of  filter  order  is  more  difficult  since  low  filter  order  is  appropriate 
for  some  segments  while  high  filter  order  is  appropriate  for  other  segments.  It  is  anticipated  that 
future  (adaptive)  algorithms  will  provide  better  performance  not  only  for  paths  with  some 
constant  curvature,  but  also  for  paths  with  varying  curvature. 

4.4.  Impact  of  Cart  Motion  Characterization 

The  impact  of  characterizing  the  cart  path  prior  to  applying  error-correction  algorithms  to  the 
GPS  measurements  is  evaluated  through  simulations.  For  comparison,  an  LPF  of  order  6  is  also 
evaluated  in  these  simulations.  The  cart  path  utilized  in  the  simulations  is  the  simulated  path 
shown  in  Figure  3.  In  these  simulations,  the  error  variances  in  the  x-  and  y-directions  range  from 
a2  =  lxlO’6  to  a2  =  lxlO’3.  These  error  levels  are  smaller  than  those  used  previously  because  the 
distance  between  successive  samples  is  now  .05m,  rather  than  lm. 

First,  the  HMM  is  applied  to  the  first  difference  in  the  GPS  measurements  to  estimate  the  state 
sequence.  The  most  likely  state  sequence  is  detennined  using  the  Viterbi  algorithm,  and  that 
state  sequence  is  then  used  to  guide  the  error  correction.  If  the  estimated  state  is  one  of  the 
forward  motion  states,  an  LPF  of  order  6  is  applied  to  reduce  the  GPS  measurement  errors.  The 
filter  order  is  reduced  at  the  boundaries  of  the  cart  path  segment  so  that  measurements  associated 
with  a  different  type  of  motion  do  not  impact  the  error  correction  of  the  current  segment.  The 
estimated  state  sequence  is  used  to  define  the  boundaries  of  the  different  segments  in  the  cart 
path.  Thus,  the  HMM  not  only  allows  us  to  apply  an  appropriate  filter  to  the  different  segments 
of  the  cart  path,  but  it  also  allows  us  to  transition  between  the  segments  in  an  appropriate  way.  If 
the  estimated  state  is  one  of  the  detour  or  turn  around  states,  an  LPF  of  order  2  is  applied  to 
reduce  the  GPS  measurement  errors.  The  lower  filter  order  removes  the  major  perturbations  in 
the  GPS  measurements,  while  not  imposing  a  linear  model  over  a  long  series  of  measurements. 
This  filter  order  is  chosen  for  the  detours  because  it  is  anticipated  that  the  detours  seen  in  real 
data  will  be  more  gently  curved,  and  not  the  idealized  straight  line  segments  utilized  in  these 
simulations. 

Single  realization  simulation  results  are  shown  for  a  range  of  error  variances  in  Figures  32 
through  38  (low  variance  to  high  variance).  The  top  panel  in  each  of  these  figures  shows  the 
noisy  measurements  (green  dots),  the  corrected  measurements  using  the  HMM  (red  circles),  and 
the  corrected  measurements  using  the  LPF  (cyan  squares)  for  the  entire  cart  path.  The  bottom 
panels  each  show  sections  of  the  cart  path  in  more  detail.  The  left  bottom  panel  shows  a  turn 
around  and  perturbations  induced  by  cart  tilt,  and  the  right  bottom  panel  shows  detours  as  well  as 
a  straight  line  segment. 

The  bottom  panels  illustrate  the  differences  between  the  using  the  HMM  and  only  the  LPF  on  the 
GPS  error  correction  at  the  boundaries  between  the  straight  line  motion  and  the  nonlinear  motion 
(detour  or  turn  around).  At  low  and  moderate  noise  variances,  where  the  modeling  errors 
resulting  from  assuming  strictly  linear  motion  dominate  the  LPF  results,  the  HMM  outperfonns 
the  LPF.  At  high  noise  variances,  where  the  noise  dominates  the  LPF  results,  the  LPF 
outperforms  the  HMM.  The  exception  to  these  observations  is  where  cart  tilt  introduces 
perturbations  in  the  cart  path.  At  these  locations,  the  LPF  outperforms  the  HMM  for  all  error 
levels  because  the  HMM  is  incorrectly  characterizing  these  segments  as  small  detours.  This 
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illustrates  the  sensitivity  of  the  HMM  approach  to  the  accuracy  of  the  model.  If  the  detour  model 
forced  the  detours  to  be  at  least  farther  away  from  the  forward  motion  segment  than  the 
perturbation  induced  by  the  cart  tilt,  it  is  likely  those  segments  would  be  correctly  characterized 
as  forward  motion.  This  hypothesis  will  be  evaluated  in  future  simulations. 

The  above  observations  are  quantified  in  Figure  39.  The  figure  summarizes  the  results  obtained 
for  100  independent  realizations  of  noisy  GPS  measurements.  It  shows  the  mean  RMS  error 
across  all  measurements  as  a  function  of  the  GPS  measurement  error  variance.  The  mean  RMS 
errors  increase  as  the  measurement  error  variance  increases,  as  expected.  It  is  interesting  to  note 
that  the  HMM  has  lower  mean  RMS  error  than  the  LPF  for  low  measurement  error  variance; 
however,  the  LPF  has  lower  mean  RMS  error  than  the  HMM  for  high  measurement  error 
variance.  These  are  consistent  with  the  observations  made  about  the  corrected  cart  paths  for  the 
single  realization  results. 


5.  Real  Data  Processing:  ATD/JPG 

The  GPS  error  correction  algorithm  is  validated  using  data  collected  at  the  ATD/JPG,  and  the 
results  are  shown  in  Figure  40.  Some  of  the  algorithm  parameters  were  adjusted  to  better  match 
the  measurement  conditions  in  this  data  collection.  For  instance,  the  distance  between  successive 
measurements  was  increased  from  0.05m  to  0.125m.  In  addition,  the  cart  motion  model  was 
modified  to  force  the  detours  to  be  further  away  from  the  straight  line  path  by  adding  more  states 
to  the  movement  away  from  and  back  toward  the  forward  motion  state. 

While  there  were  volumes  of  data  collected,  and  processed,  only  a  representative  subset  is  shown 
in  Figure  40.  This  section  of  the  cart  path  was  chosen  because  it  illustrates  some  interesting 
features.  Since  ground  truth  is  not  available  for  this  data  set,  the  performance  evaluation  is  more 
qualitative  than  quantitative.  In  the  section  at  approximately  (x,y)  =  (-0.75,20.75),  both  the  LPF 
and  the  HMM  followed  by  filtering  remove  the  bump  seen  in  the  measured  data.  This  is  a  section 
where  the  HMM  characterized  the  cart  motion  as  linear,  and  attributes  the  deviation  in  the  GPS 
measurements  to  the  cart  tilting.  However,  in  the  section  at  approximately  (x ,y)  =  (-2.75,20.75), 
the  HMM  follows  the  measured  data,  while  the  LPF  alone  deviates  significantly  from  it.  In  this 
section,  the  HMM  characterized  the  cart  motion  as  the  beginning  of  the  turn  around.  Finally,  the 
turn  around  section  shows  that  the  LPF  alone  introduces  a  systematic  error,  while  the  HMM 
continues  to  follow  the  measured  data. 

Although  the  HMM  approach,  in  this  case,  appears  qualitatively  to  have  done  a  better  job  at 
error  correcting  than  the  LPF  alone,  the  approach  is  far  from  robust  in  its  current  configuration. 
The  major  source  of  contention  is  usually  the  incorrect  characterization  of  perturbations  in  the 
cart  path  (due  to  tilting)  as  small  detours  in  the  cart  path.  A  logical  way  to  resolve  this  ambiguity 
is  to  track  the  orientation  of  the  cart  as  it  moves  over  the  uneven  terrain. 
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y-Coordinate  [m] 


Corrected  Cart  Path  (noise  variance  =  1e-006) 


Corrected  Cart  Path  (noise  variance  =  1e-006) 


Corrected  Carl  Path  (noise  variance  -  1e-006) 


(b)  Detail  of  turn  around  and  cart  tilt. 


(c)  Detail  of  obstacle  avoidance  and  straight  line. 


Figure  32:  Simulated  cart  path  with  noisy  GPS  measurements  and  error-corrected  GPS  measurements 

for  measurement  error  variance  o2  =  lx  10"6. 
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y-Coordmale  [m] 


Corrected  Cart  Path  (noise  variance  =  5e-006) 


(a)  Corrected  cart  path. 


Corrected  Cart  Path  (ncuse  variance  ■  5e-006) 


(b)  Detail  of  turn  around  and  cart  tilt. 


(c)  Detail  of  obstacle  avoidance  and  straight  line. 


Figure  33:  Simulated  cart  path  with  noisy  GPS  measurements  and  error-corrected  GPS  measurements 

for  measurement  error  variance  cr2  =  5  x  10  s. 
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Corrected  Cart  Path  (noise  variance  =  1e-005) 


Corrected  Cart  Path  (noise  variance  =  1e-005) 


Corrected  Cart  Path  (noise  variance  =  1e-005) 


(b)  Detail  of  turn  around  and  cart  tilt. 


(c)  Detail  of  obstacle  avoidance  and  straight  line. 


Figure  34:  Simulated  cart  path  with  noisy  GPS  measurements  and  error-corrected  GPS  measurements 

for  measurement  error  variance  a2  =  1  x  10  s. 


y-Ccxxdinate  [m] 


Corrected  Cart  Path  (noise  variance  =  5e-005) 


Corrected  Cart  Path  (noise  variance  =  5e-005) 


(b)  Detail  of  turn  around  and  cart  tilt. 


Corrected  Cart  Path  (noise  variance  =  5e-005) 


(c)  Detail  of  obstacle  avoidance  and  straight  line. 


Figure  35:  Simulated  cart  path  with  noisy  GPS  measurements  and  error-corrected  GPS  measurements 

for  measurement  error  variance  a2  =  5  x  10  ~4. 
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y- Coordinate  [m| 


Corrected  Cart  Path  (noise  variance  =  0.0001) 


Corrected  Cart  Path  (noise  variance  =  0.0001) 


Corrected  Cart  Path  (noise  variance  «  0.0001) 


(b)  Detail  of  turn  around  and  cart  tilt. 


(c)  Detail  of  obstacle  avoidance  and  straight  line. 


Figure  36:  Simulated  cart  path  with  noisy  GPS  measurements  and  error-corrected  GPS  measurements 

for  measurement  error  variance  a2  =  1  x  10 "4. 
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Corrected  Cart  Path  (noise  variance  =  0.0005) 


Corrected  Cart  Path  (notse  variance  =  0.0005) 


(b)  Detail  of  turn  around  and  cart  tilt. 


(c)  Detail  of  obstacle  avoidance  and  straight  line. 


Figure  37:  Simulated  cart  path  with  noisy  GPS  measurements  and  error-corrected  GPS  measurements 

for  measurement  error  variance  a2  =  5  x  10'3. 
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y-Coordinale  [m] 


Corrected  Cart  Path  (noise  variance  =  0.001) 


Simulated  GPS  Measurements 
Noisy  GPS  Measurements 
O  Corrected  Measurements  (HMM+LPF) 

Corrected  Measurements  (LPF) 

i  i  i  _ i _ i _ L 

0  1  2  3  4  5 

x-Coordinate  [m] 

(a)  Corrected  cart  path. 


2.5 


0.5 


-0.5 


(b)  Detail  of  turn  around  and  cart  tilt.  (c)  Detail  of  obstacle  avoidance  and  straight  line. 


Figure  38:  Simulated  cart  path  with  noisy  GPS  measurements  and  error-corrected  GPS  measurements 

for  measurement  error  variance  c2  =  lx  10"3. 
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Mean  RMS  Error  in  Distance 


Figure  39:  Mean  RMS  error  across  all  measurements  as  a  function  of  GPS  measurement  error  variance. 


22.5 


20 


Measured  GPS  Data 

O  Corrected  Measurements  (HMM+LPF) 
Corrected  Measurements  (LPF) 

-3.5  -3  -2.5  -2  -1.5  -1  -0.5  0 

x-Coordinate  [m] 


Figure  40:  Error  correction  algorithms  applied  to  data  collected  at  the  JPG  V  demonstration. 
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6.  Three-Dimensional  Cart  Motion 


6.1.  Quaternions 

When  dealing  with  the  cart  motion  in  three  dimensions,  the  common  approach  is  to  use  a 
sequence  of  three  Euler  angle  rotations  to  completely  define  the  orientation  of  the  cart  relative  to 
a  reference  coordinate  system  at  each  point  along  the  cart  path.  In  general,  any  angle  of  rotation 
about  a  coordinate  axis  is  called  an  Euler  angle.  One  such  sequence  that  is  often  used  is  the 
yaw(r/f)-pitch(W)-roll(  y/)  sequence  of  Figure  41.  In  describing  the  evolution  of  the  cart  motion, 
one  must  keep  track  of  the  evolution  of  the  location  (x,  y,  z)  and  all  three  Euler  angles  ((f),  0,  y/). 


YAW  (<p)  -  rotation  about  PITCH  (6)  -  rotation  about  ROLL  (ip)  —  rotation  about 

the  z-axis  the  new  y-axis  y'  the  last  x-axis  x" 

Figure  41:  Yaw-Pitch-Roll:  A  common  Euler  angle  sequence. 


If  the  cart  motion  were  restricted  to  a  plane,  the  state  of  each  point  would  be  completely  defined 
by  its  location  on  the  plane  (x,  y)  and  the  Euler  angle  (f) .  In  this  case,  a  recursive  relation  for  the 
evolution  of  (f)  can  be  written  simply  as 


<t>„  =  </>„- 1  +  »  n  =  1,2,3... 

i=i 

where  A</>n  =  <f>n  -<f>n_x  represents  the  incremental  change  in  ^  between  points  n- 1  and  n,  and  (f>{) 

represents  the  initial  angle.  This  same  recursive  representation  for  an  angle,  however,  is  a  natural 
consequence  of  multiplying  complex  numbers  (of  unit  magnitude)  in  the  polar  representation. 
Thus,  if  we  represent  the  complex  number  c  as 

c  =  a  +  ib  =  (r  cos  <f))  +  i(r  sin  (j>)  =  re1'1’ , 


where 
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=  |c|  =  sla2  +  b 2  and  (f)  = 


tan 


then  the  recursive  product  of  unit  complex  numbers  gives 


/=i 


The  above  result  reinforces  the  idea  that  given  a  point  on  the  unit  circle  in  the  complex  plane 
(i.e.  c„_j ,  which  represents  the  angle  (j)n  , ),  we  can  multiply  it  by  another  unit  complex  number 

(i.e.  Ac,, )  in  order  to  rotate  it  to  a  new  point  on  the  unit  circle  (i.e.  cn ,  which  now  represents  the 
angle  (j>n ).  In  other  words,  points  on  the  unit  circle  in  the  complex  plane  represent  all  allowable 

rotations  of  the  cart  frame,  provided  0=  y/=  0  (i.e.  the  cart  motion  is  restricted  to  a  plane),  and 
furthermore  the  evolution  of  the  rotation  angle  (f>  is  completely  specified  as  a  path  along  the  unit 
circle. 


If  we  now  relax  the  condition  that  the  cart  motion  be  restricted  to  a  plane,  it  turns  out  that  a 
generalization  of  the  above  exists:  points  on  a  unit  hyper-sphere  in  four-dimensional  complex 
space  represent  all  allowable  rotations  in  three  dimensions,  and  furthermore  the  evolution  of  the 
rotation  Euler  angles  (<p,  0,  i//)  is  completely  specified  by  a  path  on  the  unit  hyper-sphere.  This  is 
done  by  extending  the  above  idea  for  complex  numbers,  to  something  called  quaternions  [10- 
11]  -  sometimes  also  called  hyper-complex  numbers  of  rank  4  -  where  the  multiplication 
operator  no  longer  obeys  the  commutative  law  (i.e.  the  multiplication  sequence  is  important). 
Thus,  if  we  represent  the  quaternion  q  as 

q  =  u  +  ivj  +  jv2  +  kv3  =  u  +  v , 


where  u  is  the  scalar  real  part  of  the  quaternion  while  v  is  the  vector  imaginary  part  of  the 
quaternion,  and  the  vector  components  [vi,  V2,  V3]  are  mutually  orthogonal  with  the  standard 
directional  unit  vectors  i ,  j  and  k  ,  respectively;  then  the  unit  quaternion  can  be  expressed  in  a 
polar-like  representation  as 


C  ~  .  C 

q  =  u  +  \  =  cos  —  +  E  sin  — , 

2  2 

where  E  represents  a  unit  vector  about  which  a  positive  rotation  of  f  (as  defined  by  a  right- 
handed  coordinate  system)  takes  place.  This  interpretation  stems  from  the  quaternion  product  of 
q°r°q*,  a  quantity  referred  to  as  the  quaternion  rotation  operator,  where  r  is  any  three- 
dimensional  vector  to  be  rotated  and  q*  =u  —  \  is  the  complex  conjugate  of  the  unit  quaternion 

q.  Thus,  assuming  r  =  i  and  E  =  k ,  and  noting  that  the  quaternion  product  has  the  following 
properties: 
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i°i  =  j°j  =  k°k  =  iojok  =  -l; 


we  have 

q  o  r  o  q*  =  i  cos^  +  j  sin^  =  rrotated , 


where  it  is  clear  that  the  quaternion  rotation  operator  has  rotated  the  vector  r  =  i  by  a  positive 
angle  C,  about  the  E  =  k  axis. 

Returning  to  the  problem  of  describing  the  evolution  of  the  cart  motion  in  three  dimensions,  we 
first  note  that  due  to  a  theorem  by  Euler,  any  given  sequence  of  rotations  can  be  represented  as  a 
single  rotation  about  a  single  fixed  axis.  The  (ft-O-y/  sequence  can  therefore  be  represented  by  a 
single  rotation  about  a  single  axis,  say  f  about  E  ,  which  we  have  seen  can  be  readily  expressed 

as  a  unit  quaternion  q.  If  we  now  track  from  point  to  point  the  vector  E  ,  for  example,  we  have 
the  recursive  relation 


E„  =  ‘I,,  °  V  "  q'„  =  q,  "  (<?„_,  °  E„_,  ° q’„  )  °  <?,'=(«„  °  ) °  E„_,  o  (</„  o  )' 

=  (li<?,>°Et°(ri<?,)‘  >  "  =  1.2,3... 


where  qn  is  the  unit  quaternion  that  is  needed  to  rotate  the  En  ,  vector  into  En ,  and 


ru  i°-°ft°?o 

1=1 

is  itself  the  unit  quaternion  needed  to  rotate  the  initial  vector,  E0 ,  into  En .  Note  that  q0 

represents  the  rotation  that  takes  E0  into  E, ;  q \  represents  the  rotation  that  takes  E,  into  E, ; 

etc...  It  is  clear,  therefore,  that  the  rotational  evolution  of  a  vector  is  efficiently  described 
through  a  quaternion  product  of  unit  quaternions  representing  incremental  rotations  at  each  point 
along  the  track,  and  furthermore  that  the  product  outcome  is  itself  a  unit  quaternion  that 
represents  the  combined  rotation  from  start  to  finish. 

At  this  point,  it  is  important  to  recognize  that  unit  quaternions  -  which  can  be  thought  of  as 
points  on  a  unit  hyper-sphere  in  four  dimensions  -  provide  an  unambiguous  mapping  to  three- 
dimensional  rotations.  For  each  rotation,  however,  there  exists  two  corresponding  unit 
quaternions,  q  and  -q.  Topologically,  quaternion  space  consists  of  a  “double  covering”,  where  q 
maps  to  one  covering  and  -q  maps  to  the  other.  Any  continuous  quaternion  curve  on  a  single 
covering  corresponds  to  a  continuous  sequence  of  rotations  [12].  In  this  case,  we  can  always 
define  a  great  circle  as  the  unit  circle  that  results  from  the  intersection  of  the  unit  sphere  in  three- 
dimensional  real  projective  space  with  a  two-dimensional  plane  containing  any  two  unit 
quaternions  belonging  to  the  curve,  as  well  as  the  origin  of  the  sphere.  There  then  exists  a 
unique  shortest  arc  along  the  great  circle  between  the  two  quaternions,  and  so  the  angle  that 
subtends  this  arc  becomes  a  natural  metric  to  use  in  order  to  measure  the  distance  between  two 
unit  quaternions.  Indeed,  if  qo  and  qi  are  two  unit  quaternions,  and  qo  rotates  to  qi ,  then 
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<h  =(?1°^)°?0 

(since  q*0  °  q0  =  u2  +  v  •  v  =  1  for  unit  quaternions)  and  the  composite  quaternion  qx  °g*  is  the 
rotation  connecting  them.  And  since 

?i°«o  =«i«o  +  vrv0  =9i 

where  g,  •  g0  is  the  four-dimensional  dot  product  of  qi  and  qo  ,  this  is  by  definition  the  cosine  of 
the  angular  separation  between  q0  and  qi  on  the  unit  sphere. 


In  summary: 

•  Any  three-dimensional  rotation  can  be  represented  by  one  of  two  unit  quaternions  (q  or  - 
q). 

•  All  unit  quaternions  reside  on  a  hyper-sphere  in  four  dimensions.  Topologically,  they  can 
reside  on  one  of  two  coverings. 

•  Any  continuous  quaternion  curve  on  a  single  covering  corresponds  to  a  continuous 
sequence  of  rotations,  and  has  the  geometry  of  three-dimensional  real  projective  space 

•  The  quaternion  product  of  a  sequence  of  points  on  a  curve  on  the  unit  sphere  is  itself 
another  point  on  the  unit  sphere,  and  represents  the  combined  rotation  from  start  to  finish 
for  that  sequence  of  points. 

•  The  quaternion  product  qn  °  qn_x  connects  point  qn  ,  to  qn  on  the  unit  sphere,  and 
represents  the  cosine  of  the  angular  separation  between  qn  ,  and  qn  on  the  unit  sphere, 
thus  giving  the  product  a  geometrical  interpretation. 


6.2.  Advantages  of  Using  Quaternions 

It  was  stated  that  when  dealing  with  the  cart  motion  in  three  dimensions,  the  common  approach 
is  to  use  a  sequence  of  three  Euler  angle  rotations  (such  as  the  sequence  of  Figure  40)  to 
completely  define  the  orientation  of  the  cart  relative  to  a  reference  coordinate  system  at  each 
point  along  the  cart  path.  In  this  common  approach,  all  rotations  are  represented  by  3  x  3 
orthogonal  matrices  whose  determinants  are  +1  (subsequently  to  be  referred  to  as  rotation 
matrices).  The  matrix  product  -  also  non-commutative  -  of  a  sequence  of  rotation  matrices  also 
results  in  a  rotation  matrix  that  represents  the  combined  rotation  of  the  sequence  from  start  to 
finish.  Thus,  in  an  analogous  fashion  to  unit  quaternions,  a  recursive  matrix  relation  expressing 
the  rotational  evolution  of  the  cart  frame  can  be  formulated.  Also  analogous  to  unit  quaternions, 
a  matrix  product  connecting  two  rotation  matrices  exists.  However,  unlike  the  unit  quaternions,  a 
direct  geometrical  interpretation  linking  the  two  matrices  does  not  exist. 

There  in  lies  the  greatest  advantage  of  using  the  quaternion  formulation  over  the  more  common 
matrix  formulation.  By  knowing  the  topology  and  geometry  that  links  a  sequence  of  unit 
quaternions  together,  it  is  possible  to  manipulate  the  orientation  evolution  of  the  cart  frame  with 
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a  lot  more  efficiency  and  dexterity.  The  cost,  however,  is  that  the  new  landscape  has  to  be 
explored  and  a  degree  of  familiarization  attained  before  this  advantage  can  be  exploited. 

Take,  for  example,  the  interpolation  of  orientation  of  the  cart  body  frame  given  sparse  data 
sampling  in  the  form  of  the  Euler  angles  <f>,  0,  y/.  In  the  matrix  formulation,  there  is  no 
straightforward  method  of  finding  intennediate  rotation  matrices  given  end  point  rotation 
matrices.  It  is  not  a  simple  matter  of  interpolating  the  Euler  angles  separately  since  these  angles 
are  obviously  correlated.  In  the  quaternion  formulation,  however,  the  geometrical  interpretation 
connecting  the  two  unit  quaternions  allows  one  to  intuitively  interpolate  between  the  two  points. 
One  simple  scheme  is  the  spherical  linear  interpolation  -  abbreviated  as  slerp  -  which  linearly 
interpolates  along  the  great  arc  connecting  the  two  points  and  represents  a  constant  speed 
rotation  between  the  points  [13-14],  Many  more  elaborate  schemes  also  exist  [15-16]. 

By  now  it  should  be  clear  that  any  filtering  (i.e.  LPF,  linear  predictive,  etc...)  of  the  orientation 
data  needs  to  take  place  in  the  nonlinear  unit  quaternion  space.  Failing  to  do  so  will  result  in 
non-intuitive  methods  involving  nonlinear  transformations,  and  will  therefore  be  more  prone  to 
errors.  Using  linear  approximations  might  simplify  the  process,  but  this  then  has  the  potential  of 
resulting  in  significant  deviations  from  its  true  behavior  if  the  orientation  errors  get  large. 

Another  advantage  of  using  quaternions  -  one  that  becomes  more  of  an  issue  when  considering 
real-time  processing  -  is  the  fact  that  the  only  time  we  need  to  compute  the  rotation  matrix  is 
when  we  need  to  transform  to  the  cart  frame.  For  most  other  operations,  such  as  the  filtering 
schemes  referred  to  above,  we  need  only  look  at  the  quaternions.  Since  a  matrix  product  requires 
many  more  operations  than  a  quaternion  product,  we  can  save  a  lot  of  time  and  preserve  more 
numerical  accuracy  with  quaternions  than  with  matrices  [17]. 

6.3.  A  Quaternion  Formulation  Using  GPS  Measurements 

This  formulation  closely  follows  one  suggested  by  Hanson  [18].  His  method  is  applied  to  the 
idea  of  describing  the  full  cart  motion  -  orientation  included  -  using  only  GPS  ( x,y,z ) 
measurements.  The  steps  are  enumerated  below: 

1 .  Create  a  curve  in  space  which  best  represents  the  location  of  the  cart  at  each  point:  use  GPS 
data  for  this.  This  curve  should  be  described  in  the  tangent-plane  reference  frame  (i.e.  earth); 

2.  At  each  point  along  the  curve,  compute  a  unit  tangent  vector  T  .  This  represents  the  heading 
at  each  point,  and  will  be  tied  to  the  x-axis  of  the  cart-body  reference  frame. 

3.  If  is  the  quaternion  that  rotates  T  ,  to  T. ,  then  the  recursive  relation  for  the  evolution  of 
T  is  simply 

t=(n«,)°t>»(rW  •  n=i’2’3"' 

i=l  i= 1 

4.  Note,  however,  that  any  quaternion  p,(%)  can  replace  the  quaternion  qt  above,  provided 
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P /  (4)  =  fe) °  Vi  =  (cos  |  + 1  sin  |)  °  q.  , 

since  (c)  is  the  quaternion  that  leaves  the  direction  T  invariant.  The  angle  Ig  serves  to 

parameterize  a  ring  in  quaternion  space,  each  point  of  which  corresponds  to  a  particular  cart- 
body  frame  at  (. xuyuz and  all  frames  are  described  twice  since  0  <  g  <  4/r .  p^g)  is  thus 
known  as  the  sliding  ring  constraint,  for  obvious  reasons. 

5.  Finally,  choose  the  angle  g  al  every  cart  location  by  minimizing  the  quaternion  length  of  the 
frame  assignment  for  the  curve.  The  justification  for  this  is  that  there  is  a  unique  rotation  in 
the  plane  of  two  neighboring  tangents  that  takes  each  tangent  direction  to  its  next  neighbor 
along  a  curve  -  this  is  the  geodesic  arc  connecting  the  two  frames  in  quaternion  space,  and  is 
therefore  the  minimum  distance  between  the  quaternion  points  representing  the  two  frames. 

This  last  step  represents  a  nontrivial  step  which  is  highly  dependent  on  the  initial  orientation 
chosen  for  the  cart. 


7.  Summary 

The  sensitivity  of  EMI  dipole  and  magnetometer  dipole  model  parameter  estimates  to  errors  in 
the  GPS  measurements  was  evaluated  through  simulations.  The  results  suggest  that  decreasing 
the  error  in  the  GPS  measurements  tends  to  decrease  the  error  in  the  parameter  inversion,  with 
the  degree  of  improvement  dependent  upon  the  target  parameters  and  orientation  relative  to  the 
data  collection.  This  observation  motivates  the  need  for  techniques  to  reduce  GPS  measurement 
errors.  A  hidden  Markov  model  for  the  cart  motion  has  been  implemented  to  broadly 
characterize  the  cart  motion  as  linear  or  nonlinear.  The  HMM  characterizes  the  cart  path  so  that 
different  error  correction  techniques  can  be  applied  to  different  segments  of  the  path.  For 
instance,  the  error  correction  applied  to  the  linear  segments  is  different  from  the  error  correction 
applied  to  the  nonlinear  segments. 

Simulations  were  performed  to  compare  the  performance  of  the  HMM  followed  by  segment 
specific  filtering,  to  the  LPF  alone.  The  results  show  that  for  low  and  moderate  error  levels, 
where  the  GPS  errors  do  not  impede  cart  motion  characterization,  the  HMM  in  general  offers 
better  performance.  The  better  performance  is  due  in  large  part  to  the  improved  performance  in 
the  regions  where  the  motion  is  nonlinear.  However,  for  large  error  variance,  where  the  GPS 
errors  prevent  accurate  characterization  of  the  cart  motion,  the  LPF  alone  offers  better 
performance.  The  same  algorithms  were  then  applied  to  field  data  collected  at  the  ATD/JPG. 

A  potential  problem  for  the  HMM  is  in  the  characterization  of  perturbations  in  the  cart  path  due 
to  tilting  of  the  cart.  Such  segments  in  the  cart  path  are  occasionally  mistaken  for  small  detours. 
To  address  this  deficiency,  a  quaternion  formulation  has  been  proposed  as  a  possible  efficient 
means  to  track  and  process  the  cart  orientation  as  it  moves  over  the  uneven  terrain.  With  this 
added  orientation  information,  ambiguities  encountered  when  attempting  to  characterize  the  cart 
motion  as  being  either  linear  or  nonlinear  can  be  resolved,  offering  an  opportunity  for  improved 
robustness  for  the  HMM  approach. 
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8.  Directions  for  Future  Research 


There  are  several  directions  for  continued  and  future  research  related  to  this  work.  One  obvious 
direction  is  to  incorporate  the  proposed  quaternion  formulation  into  the  HMM  to  improve  the 
robustness  of  the  currently  implemented  approach.  This  will  naturally  lead  to  questions 
regarding  estimates  of  the  initial  orientation  state  and  whether  an  additional  sensor,  such  as  an 
inertial  motion  unit  (IMU),  will  actually  be  necessary  to  make  this  work.  If  an  IMU  is  deemed 
necessary,  there  is  then  the  question  of  whether  it  makes  more  sense  to  go  with  something  more 
sophisticated,  such  as  a  Kalman-type  filter,  to  combine  the  GPS  and  IMU  data.  Like  the  current 
implementation  of  the  HMM  with  an  LPF  of  adaptively  varying  order,  the  adaptation  of  the 
Kalman  filter  could  be  guided  or  influenced  by  the  cart  motion  characterization  determined  using 
the  improved  HMM. 

Another  direction,  which  will  be  site  dependent,  might  be  to  closely  examine  the  relationship 
between  the  characteristics  of  the  ground  surface  and  the  distributions  of  GPS  errors.  The 
relationship  between  the  ground  surface  and  GPS  error  distributions  could  then  be  utilized  to 
guide  the  design  of  the  HMM  for  the  cart  motion  model. 
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